-module(collision).
-export([segment_collides/4,
         line_collides/4,
         can_see/3,
         triangles_in_bbox/2,
         bboxes_involved_in_segment/3]).

%% @spec intersect_plane(Origen::vec3(), Destino::vec3(), PuntoEnPlano::vec3(), NormalPlano::vec3()) -> {parallel} | {intersect, Posicion::float()}
%% @doc Posicion en la que la linea de Origen a Destino toca al plano, en donde el Origen es 0 y el destino es 1.
intersect_plane(L0 = {vec3, _, _, _}, L1 = {vec3, _, _, _}, Pp = {vec3, _, _, _}, Np = {vec3, _, _, _}) ->
    %Sup = Np dot (Pp - L0)
    D1 = vector:rest(Pp, L0),
    Sup = vector:dot(Np, D1),
    %V = L1 - L0
    V = vector:rest(L1, L0),
    %Sub = Np dot V
    Sub = vector:dot(Np, V),
    case Sub of
        0.0 ->
            {parallel};
        _ ->
            {intersect, Sup / Sub}
    end.

%% @spec position_line(Origen::vec3(), Destino::vec3(), Posicion::float()) -> Punto::vec3()
%% @doc Posicion de un punto T sobre la linea Origen a Destino, en donde el Origen es 0 y el destino es 1.
position_line(L0 = {vec3, _, _, _}, L1 = {vec3, _, _, _}, T) ->
    V = vector:rest(L1, L0),
    vector:sum(L0, vector:mult(V, T)).

%% @spec line_plane_intersection(Origen::vec3(), Destino::vec3(), PuntoPlano::vec3(), NormalPlano::vec3()) -> {parallel} | {behind, Posicion::float()} | {ahead, Posicion::float()} | {inside, Posicion::float()}
%% @doc Posicion y condiciones en la que una linea intersecta un plano.
line_plane_intersection(L0 = {vec3, _, _, _}, L1 = {vec3, _, _, _}, Pp = {vec3, _, _, _}, Np = {vec3, _, _, _}) ->
    T = intersect_plane(L0, L1, Pp, Np),
    case T of
        {intersect, N} when N < 0.0 ->
            {behind, position_line(L0, L1, N)};
        {intersect, N} when N > 1.0 ->
            {ahead, position_line(L0, L1, N)};
        {intersect, N} ->
            {inside, position_line(L0, L1, N)};
        {parallel} ->
            {parallel}
    end.

%% @spec internal_side(Punto::vec3(), A::vec3(), B::vec3(), C::vec3()) -> bool()
%% @doc Verifica si P1 esta dentro del lado dado por A, B y C
internal_side(P1 = {vec3, _, _, _}, P2 = {vec3, _, _, _}, A = {vec3, _, _, _}, B = {vec3, _, _, _}) ->
    Vb = vector:rest(B, A),
    S1 = vector:rest(P1, A),
    S2 = vector:rest(P2, A),
    Cp1 = vector:cross(Vb, S1),
    Cp2 = vector:cross(Vb, S2),
    vector:dot(Cp1, Cp2) > 0.

%% @spec point_in_triangle(Punto::vec3(), Triangulo::triangle()) -> bool()
%% @doc Verifica si un punto se encuentra dentro de un triangulo
point_in_triangle(P = {vec3, _, _, _}, {A = {vec3, _, _, _}, B = {vec3, _, _, _}, C = {vec3, _, _, _}}) ->
    R1 = internal_side(P, A, B, C),
    case R1 of
        true -> 
            R2 = internal_side(P, B, A, C),
            case R2 of
                true ->
                    internal_side(P, C, A, B);
                _ ->
                    false
            end;
        _ ->
            false
    end.

%% @spec segment_collides(Origen::vec3(), Destino::vec3(), Triangulo::triangle(), Normal::vec3()) -> bool()
%% @doc Verifica si el segmento Origen-Destino intersecta al triangulo
segment_collides(L0 = {vec3, _, _, _}, L1 = {vec3, _, _, _}, T = {A = {vec3, _, _, _}, _B = {vec3, _, _, _}, _C = {vec3, _, _, _}}, N = {vec3, _, _, _}) ->
    P = line_plane_intersection(L0, L1, A, N),
    case P of
        {inside, V = {vec3, _, _, _}} ->
            point_in_triangle(V, T);
        _ ->
            false
    end.

%% @spec line_collides(Origen::vec3(), Destino::vec3(), Triangulo::triangle(), Normal::vec3()) -> bool()
%% @doc Verifica si la linea Origen-Destino extendiendose desde -infinito hasta +infinito intersecta al triangulo
line_collides(L0 = {vec3, _, _, _}, L1 = {vec3, _, _, _}, T = {A = {vec3, _, _, _}, _B = {vec3, _, _, _}, _C = {vec3, _, _, _}}, N = {vec3, _, _, _}) ->
    P = line_plane_intersection(L0, L1, A, N),
    case P of
        {_, V = {vec3, _, _, _}} ->
            point_in_triangle(V, T);
        _ ->
            false
    end.

%% @spec can_see(Origen::vec3(), Destino::vec3(), Triangulos::triangleList()) -> bool()
%% @doc Verifica si desde el Origen se puede llegar al Destino sin ningun triangulo obstruyendo
can_see({vec3, _, _, _}, {vec3, _, _, _}, []) ->
    true;
can_see(P0 = {vec3, _, _, _}, P1 = {vec3, _, _, _}, [{T = {{vec3, _, _, _}, {vec3, _, _, _}, {vec3, _, _, _}},N = {vec3, _, _, _}} | L]) ->
    VecN = vector:normalize(vector:rest(P1, P0)),
    case vector:dot(VecN, N) of
        Cos when Cos >= 0.0 ->
            R = segment_collides(P0, P1, T, N),
            case R of
                false ->
                    can_see(P0, P1, L);
                true ->
                    false
            end;
        _ -> 
            can_see(P0, P1, L)
    end.
	
%% @spec triangle_collides_bbox(BoundingBox::bbox(), Triangle::triangle()) -> bool()
%% @doc Verifica si un triangulo se intersecta con un cubo
triangle_collides_bbox({{vec3, BPx, BPy, BPz}, Width}, {{vec3, Ax, Ay, Az}, {vec3, Bx, By, Bz}, {vec3, Cx, Cy, Cz}}) ->
    {Xmin, Ymin, Zmin, Xmax, Ymax, Zmax} = {BPx, BPy, BPz, BPx + Width, BPy + Width, BPz + Width},
    {TxMin, TxMax} = util:find_min_max([Ax, Bx, Cx]),
    {TyMin, TyMax} = util:find_min_max([Ay, By, Cy]),
    {TzMin, TzMax} = util:find_min_max([Az, Bz, Cz]),
    case TxMax < Xmin orelse TxMin > Xmax orelse TyMax < Ymin orelse TyMin > Ymax orelse TzMax < Zmin orelse TzMin > Zmax of
        true ->
            false;
        _ ->
            true
    end.

%% @spec triangles_in_bbox(BoundingBox::bbox(), Triangulos::triangles()) -> Resultado::triangleList()
%% @doc Regresa los triangulos que se encuentran dentro de la caja
triangles_in_bbox(_, []) ->
    [];
triangles_in_bbox(BBox = {{vec3, _, _, _}, _}, [CTri = {Tri = {{vec3, _, _, _}, {vec3, _, _, _}, {vec3, _, _, _}}, {vec3, _, _, _}} | T]) ->
    case triangle_collides_bbox(BBox, Tri) of
        true ->
            [ CTri | triangles_in_bbox(BBox, T) ];
        _ ->
            triangles_in_bbox(BBox, T)
    end.

%% @spec bboxes_involved_in_segment(Origen::vec3(), Destino::vec3(), Triangulos::bboxTriangleList()) -> [BoundingBoxesIds::any()]
%% @doc Calcula los Bounding Boxes involucrados en una linea Origen-Destino
bboxes_involved_in_segment(P0, P1, BBoxes) ->
    bboxes_involved_in_segment(1, P0, P1, BBoxes).

bboxes_involved_in_segment(_, _, _, []) ->
    [];
bboxes_involved_in_segment(Id, P0 = {vec3, _, _, _}, P1 = {vec3, _, _, _}, [ BBox = {{vec3, _, _, _}, _} | T]) ->
    R = segment_bbox_intersection(P0, P1, BBox),
    case R of
        true ->
            [ Id | bboxes_involved_in_segment(Id + 1, P0, P1, T) ];
        false ->
            bboxes_involved_in_segment(Id + 1, P0, P1, T)
    end.

%% @spec line_bbox_intersection_point(Origen::vec3(), Destino::vec3(), D1::float(), D2::float()) -> false | vec3()
%% @doc Verifica el punto de interseccion de una linea a dos rangos
line_bbox_intersection_point(P0 = {vec3, _, _, _}, P1 = {vec3, _, _, _}, D1, D2) ->
    if
        D1 * D2 >= 0.0 ->
            false;
        D1 == D2 ->
            false;
        true ->
            poly:sum(P0, poly:mult(poly:rest(P1, P0), -D1 / (D2 - D1)))
    end.

%% @spec point_in_bbox(Punto::vec3(), Caja1::vec3(), Caja2::vec3(), Axis) -> bool()
%% @doc Verifica si un punto se encuentra dentro de una caja
point_in_bbox({vec3, Hx, Hy, Hz}, {vec3, B0x, B0y, B0z}, {vec3, B1x, B1y, B1z}, Axis) ->
    case Axis of
        x ->
            Hz > B0z andalso Hz < B1z andalso Hy > B0y andalso Hy < B1y;
        y ->
            Hz > B0z andalso Hz < B1z andalso Hx > B0x andalso Hx < B1x;
        z ->
            Hx > B0x andalso Hx < B1x andalso Hy > B0y andalso Hy < B1y;
        _ ->
            false
    end.

%% @spec segment_bbox_intersection(Origen::vec3(), Destino::vec3(), Caja::bbox()) -> bool()
%% @doc Verifica si un segmento de linea intersecta a una caja
segment_bbox_intersection(P0 = {vec3, _, _, _}, P1 = {vec3, _, _, _}, {B0 = {vec3, Bx, By, Bz}, W}) ->
    segment_bbox_intersection(P0, P1, B0, {vec3, Bx + W, By + W, Bz + W}).

segment_bbox_intersection(P0 = {vec3, P0x, P0y, P0z}, P1 = {vec3, P1x, P1y, P1z}, B0 = {vec3, B0x, B0y, B0z}, B1 = {vec3, B1x, B1y, B1z}) ->
    if
        P1x < B0x andalso P0x < B0x ->
            false;
        P1x > B1x andalso P0x > B1x ->
            false;
        P1y < B0y andalso P0y < B0y ->
            false;
        P1y > B1y andalso P0y > B1y ->
            false;
        P1z < B0z andalso P0z < B0z ->
            false;
        P1z > B1z andalso P0z > B1z ->
            false;
        P0x > B0x andalso P0x < B1x andalso P0y > B0y andalso P0y < B1y andalso P0z > B0z andalso P0z < B0z ->
            true;
        true ->
            segment_bbox_intersection_axis(P0, P1, B0, B1, {x, 0}) orelse segment_bbox_intersection_axis(P0, P1, B0, B1, {x, 1}) orelse segment_bbox_intersection_axis(P0, P1, B0, B1, {y, 0}) orelse segment_bbox_intersection_axis(P0, P1, B0, B1, {y, 1}) orelse segment_bbox_intersection_axis(P0, P1, B0, B1, {z, 0}) orelse segment_bbox_intersection_axis(P0, P1, B0, B1, {z, 1})
    end.

%% @spec segment_bbox_intersection_axis(Origen::vec3(), Destino::vec3(), Caja1::vec3(), Caja2::vec3(), Axis) -> bool()
%% @doc Verifica si una linea intersecta a una caja bajo un eje X, Y o Z
segment_bbox_intersection_axis(P0 = {vec3, P0x, P0y, P0z}, P1 = {vec3, P1x, P1y, P1z}, B0 = {vec3, B0x, B0y, B0z}, B1 = {vec3, B1x, B1y, B1z}, Axis) ->
    case Axis of
        {x, 0} ->
            S1 = line_bbox_intersection_point(P0, P1, P0x - B0x, P1x - B0x),
            case S1 of
                false ->
                    false;
                _ ->
                    point_in_bbox(S1, B0, B1, x)
            end;
        {x, 1} ->
            S2 = line_bbox_intersection_point(P0, P1, P0x - B1x, P1x - B1x),
            case S2 of
                false ->
                    false;
                _ ->
                    point_in_bbox(S2, B0, B1, x)
            end;
        {y, 0} ->
            S1 = line_bbox_intersection_point(P0, P1, P0y - B0y, P1y - B0y),
            case S1 of
                false ->
                    false;
                _ ->
                    point_in_bbox(S1, B0, B1, y)
            end;
        {y, 1} ->
            S2 = line_bbox_intersection_point(P0, P1, P0y - B1y, P1y - B1y),
            case S2 of
                false ->
                    false;
                _ ->
                    point_in_bbox(S2, B0, B1, y)
            end;
        {z, 0} ->
            S1 = line_bbox_intersection_point(P0, P1, P0z - B0z, P1z - B0z),
            case S1 of
                false ->
                    false;
                _ ->
                    point_in_bbox(S1, B0, B1, z)
            end;
        {z, 1} ->
            S2 = line_bbox_intersection_point(P0, P1, P0z - B1z, P1z - B1z),
            case S2 of
                false ->
                    false;
                _ ->
                    point_in_bbox(S2, B0, B1, z)
            end;
        _ ->
            false
    end.
